from ros2webserver.baseserver.capability import Capability
from geometry_msgs.msg import Twist
import json
from ros2webserver_msgs.srv import ProcessOp
import os

class NavStartUp(Capability):

    def __init__(self, protocol):
        Capability.__init__(self, protocol)
        protocol.register_operation("startAlgorithm", self.nav_startup)
        protocol.register_operation("stopNavigate", self.nav_stop)

        self.result = {
                "op":"startAlgorithm",
                "code":1,
                "msg":"",  
                "data":None
        }
    
    def req_process_op(self,name,op,command="",callback=None):
        self.process_op_client = self.protocol.node_handle.create_client(ProcessOp, 'process_op')
        if not self.process_op_client.wait_for_service(timeout_sec=1.0):
            self.protocol.node_handle.get_logger().info('ProcessOp service not available, waiting...')
            self.result['code'] = 1
            self.result['msg'] = 'fail'
            self.protocol.send(self.result)
        request = ProcessOp.Request()
        request.name = name
        request.op = op
        request.command = command
        future = self.process_op_client.call_async(request)
        if callback:
            future.add_done_callback(callback)
    def check_map(self,map_name):
        # 检查xxx.pgm文件是否存在,存在返回True,不存在返回False
        if os.path.exists(f'/home/yhs/{map_name}.yaml'):
            return True
        else:
            return False

    def run_navigation(self,future):
        def startup_finish(future):
            self.result['code'] = 0
            self.result['msg'] = 'ok'
            self.protocol.send(self.result)
        # self.protocol.node_handle.
        response = future.result()
        self.req_process_op('navigation',ProcessOp.Request.OP_RUN,callback=startup_finish)

    def add_navigation(self, map_name):
        command = f"ros2 launch yhs_nav2 yhs_nav2_2d.launch.py map:={map_name}"
        if not map_name:
            command = f"ros2 launch yhs_nav2 yhs_nav2_2d.launch.py"
        #command = f"ros2 launch fishbot_navigation2 navigation2.launch.py"
        self.req_process_op('navigation',ProcessOp.Request.OP_ADD,command=command,callback=self.run_navigation)

    def check_starup_status(self,map_name):
        pass
    def nav_startup(self, message):
        self.protocol.node_handle.get_logger().info(f"{message}")
        map_name= message['data']['mapName']

        # if not self.check_map(map_name):
        #     self.result['msg'] = "map not exist"
        #     self.protocol.send(self.result)
        #     return
        
        # 组合成启动导航的command,请求process_manager，启动导航，接着查询导航的状态（）
        self.add_navigation(map_name)

    def nav_stop(self,message):
        def stop_result(future):
            res = future.result()
            print(res)
            result = self.result
            result['op'] = message['op']
            result['code'] = 0
            result['msg'] = 'ok'
            self.protocol.send(result)
        self.req_process_op('navigation',ProcessOp.Request.OP_STOP,callback=stop_result)

    def finish(self):
        pass
